#include "init.h"
#include "statechanges.h"

void stateBemfSamplingEnter()
{
    extern struct state_t state;
    //extern struct setup_t setup;

    state.zc_found = 0;
    state.mode = STATE_BEMF_SAMPLING;

}

void stateStoppingEnter()
{
    extern struct state_t state;
    extern struct pid_t pid;
    state.mode = STATE_STOPPING;
    pid.i_value = 0;
    pid.output = 0;
    pid.p_value = 0;
    pid.target_rpm = 0;
}

void stateOffEnter()
{
    extern unsigned int PWM_CON_STATES[];
    extern struct pid_t pid;
    extern struct state_t state;
    state.mode = STATE_OFF;
    //extern struct setup_t setup;

    IOCON1 = PWM_CON_STATES[0]; // Overrides on, outputs off
    IOCON2 = PWM_CON_STATES[0];
    IOCON3 = PWM_CON_STATES[0];

    ADCPC0bits.TRGSRC02 = 0; //ADCPC0bits.TRGSRC0=0b00100;//Sample MC
    ADCPC0bits.TRGSRC12 = 0; //ADCPC0bits.TRGSRC1=0b00100;//Sample MB
    ADCPC1bits.TRGSRC22 = 0; //ADCPC1bits.TRGSRC2=0b00100;//Sample MA
    ADCPC2bits.TRGSRC4=0; //0b01110; //Current AN9 0b01110 = PWM Generator 1 secondary trigger selected

    ADCPC1bits.IRQEN2=0;// Interrupt Request Enable 2 bit(2)

    MDC = 0;

    pid.target_rpm = 0;
    pid.output = 0;
    pid.i_value = 0;
    pid.p_value = 0;

    state.BEMFmode = 0;
    PR3 = 65535;
    state.RPM = 0;

}

void stateAlignEnter()
{
    extern unsigned int PWM_CON_STATES[];
    extern struct state_t state;
    extern struct setup_t setup;
    extern struct pid_t pid;

    extern void configUpdateCurrentLimit();
    extern void configUpdateControlLoop();
    extern void configUpdateTimingDegrees();
    configUpdateCurrentLimit();
    configUpdateControlLoop();
    configUpdateTimingDegrees();

    state.mode = STATE_ALIGN;

    pid.p_value = 0;
    pid.i_value = setup.OPENLOOP_POWER_START; //setup.OPENLOOP_POWER_START;
    pid.output = setup.OPENLOOP_POWER_START; //pid.i_value;

    state.errorcount = 0;
    state.RPMpulseDiv = 8;
    T3CONbits.TCKPS = 1;

    IOCON1 = PWM_CON_STATES[0];
    IOCON2 = PWM_CON_STATES[4];
    IOCON3 = PWM_CON_STATES[2];

    //bemf_zc.delay = 1;

    PR3 = setup.OPENLOOP_SPEED_START;
    MDC = 0;
    
    state.step = 0;
    
    ADCPC0bits.TRGSRC02 = 1; //ADCPC0bits.TRGSRC0=0b00100;//Sample MC
    ADCPC0bits.TRGSRC12 = 1; //ADCPC0bits.TRGSRC1=0b00100;//Sample MB
    ADCPC1bits.TRGSRC22 = 1; //ADCPC1bits.TRGSRC2=0b00100;//Sample MA

    ADCPC2bits.TRGSRC4=0b01110; //Current AN9 0b01110 = PWM Generator 1 secondary trigger selected
    ADCPC1bits.IRQEN2=1;// Interrupt Request Enable 2 bit(2)
}

void stateBemfCommutationEnter()
{
    extern struct state_t state;
    state.mode = STATE_BEMF_COMMUTATION;
    state.zc_found = 1;
}

/*
void stateOpenloopEnter()
{
    MA_TRIS(1);
    extern struct state_t state;
    extern unsigned int PIDStartCounter;
    state.openloopCommutationCount = 0;
    state.mode = STATE_OPENLOOP;
    state.BEMFSTARTCOUNT = 0;
    PIDStartCounter = 0;

    ADCPC0bits.TRGSRC02 = 1; //ADCPC0bits.TRGSRC0=0b00100;//Sample MC
    ADCPC0bits.TRGSRC12 = 1; //ADCPC0bits.TRGSRC1=0b00100;//Sample MB
    ADCPC1bits.TRGSRC22 = 1; //ADCPC1bits.TRGSRC2=0b00100;//Sample MA

    ADCPC2bits.TRGSRC4=0b01110; //Current AN9 0b01110 = PWM Generator 1 secondary trigger selected
    ADCPC1bits.IRQEN2=1;// Interrupt Request Enable 2 bit(2)
}*/
